//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <ddk.h>
#include <drvpub.h>
#include <xllp_dmac.h>
#include <bulverde.h>
#include "ccamera.h"
#include <ddkit.h>
#include "ov.h"

//#define USEIPP
//#define USE_YCbCr422_PREVIEW

#define CAMDRV_BUFFERSIZE           0x200000

#define CAMDRV_PREVIEW_OFFSET       0x0
#define CAMDRV_PHOTO_OFFSET         0x071000
#define CAMDRV_PREVIEWDESC_OFFSET   0x1FD000
#define CAMDRV_PHOTODESC_OFFSET     0x1FE000

#if (_TEST_TYPE == 41)      /* Test for Runtime of ISR */
#define SAMPLETIMES 50
UInt32 ul_DmaChYIsrTime[SAMPLETIMES];
UInt32 ul_DmaChCbIsrTime[SAMPLETIMES];
UInt32 ul_DmaChCrIsrTime[SAMPLETIMES];
Int32 n_DmaChYIsrSampleCounter = 0;
Int32 n_DmaChCbIsrSampleCounter = 0;
Int32 n_DmaChCrIsrSampleCounter = 0;
#endif //_TEST_TYPE == 41

static unsigned int frameCount = 0;
static DzEvent s_stillSnapEvent;
static IEvent * s_pFrameCopyEvent;

#define DMA_CHANNEL0_FINISH 0x1
#define DMA_CHANNEL1_FINISH 0x2
#define DMA_CHANNEL2_FINISH 0x4
#define DMA_ALL_CHANNELS_FINISH \
    (DMA_CHANNEL0_FINISH | DMA_CHANNEL1_FINISH | DMA_CHANNEL2_FINISH)
static unsigned long StillDmaTransFlag = 0;
void (* s_pRotateCopy) (
    Byte *pDstY, Byte *pDstCb, Byte *pDstCr,
    Byte *pSrcY, Byte *pSrcCb, Byte *pSrcCr,
    int width, int height);

typedef ECode (*CamDrv_SetFormatFunc)(PCamDrv_Context pCameraCxt, int param);
CamDrv_SetFormatFunc s_fpFormatSet[] = {
    CamDrv_SetPhotoResolution,
    CamDrv_SetVideoResolution,
    NULL,
    CamDrv_SetDigitalZoom,
    CamDrv_SetBrightness,
    CamDrv_SetContrast,
    CamDrv_SetSharpness,
    CamDrv_SetWhiteBalance,
    CamDrv_SetColorMode,
    CamDrv_SetSceneMode,
    NULL,
    NULL,
    NULL,
    CamDrv_SetExposureCompensation,
    CamDrv_SetContinuousSnap
    };

// When digital zoom is set, the init sequence for sensor should be set again
// for better visual result, and some format will be reset.
// So we should set these format again to restore special register value.
CamDrv_SetFormatFunc s_fpFormatSetForZoom[] = {
    NULL,
    NULL,
    NULL,
    NULL,
    CamDrv_SetBrightness,
    CamDrv_SetContrast,
    CamDrv_SetSharpness,
    CamDrv_SetWhiteBalance,
    CamDrv_SetColorMode,
    CamDrv_SetSceneMode,
    NULL,
    NULL,
    NULL,
    CamDrv_SetExposureCompensation,
    NULL
    };

//extern I2C function
EXTERN_C void I2C_Initial(void);
EXTERN_C void I2C_Disable();

EXTERN_C void CIDumpRegisters();
EXTERN_C OverlayFormat *GetDisplayOverlayFormat(int layer);
EXTERN_C Boolean Display_ChangeOverlay2Format(Boolean bRecover, OverlayFormat *pTempFormat);

#ifdef DEBUG_CAMERA
static void DMADumpBufferInfo(PCamDrv_Context pCameraCxt, Boolean bPreview)
{
    unsigned int i, j;
    PCamDrv_DMABuffer pCameraBuffer;

    if (bPreview) {
        pCameraBuffer = &pCameraCxt->VideoBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->StillBuffer;
    }

    cprintf("\nDump DMA buffer info for camera:\n");
    cprintf("Buffer address:\n");
    for (i = 0; i < pCameraCxt->UsedFrames; i++) {
        for (j = 0; j < TRANS_CHANNELS; j++) {
            cprintf("(Frame %d, Channel %d)Physical: 0x%08x, Virtual: 0x%08x\n",
                i, j, pCameraBuffer->BufPhyAddr[i][j],
                pCameraBuffer->BufVirAddrKernel[i][j]);
        }
    }
    cprintf("Descriptor address:\n");
    for (j = 0; j < TRANS_CHANNELS; j++) {
        cprintf("(Channel %d)Physical: 0x%08x, Virtual: 0x%08x\n",
            j, pCameraBuffer->DescPhyAddr[j], pCameraBuffer->DescVirAddr[j]);
    }
    cprintf("FrameBufSize & FrameDescNum:\n");
    for (j = 0; j < TRANS_CHANNELS; j++) {
        cprintf("(Channel %d) FrameBufSize: %d, FrameDescNum: %d\n",
            j, pCameraBuffer->FrameBufSize[j], pCameraBuffer->FrameDescNum[j]);
    }
    cprintf("\n\n");
}

static void DMADumpDescriptors(PCamDrv_Context pCameraCxt, Boolean bPreview)
{
    unsigned int i, j, k;
    XLLP_DMAC_DESCRIPTOR_T *CurDesVirtual, *CurDesPhysical;
    PCamDrv_DMABuffer pCameraBuffer;

    if (bPreview) {
        pCameraBuffer = &pCameraCxt->VideoBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->StillBuffer;
    }

    for (i = 0; i < TRANS_CHANNELS; i++) {
        CurDesVirtual = (XLLP_DMAC_DESCRIPTOR_T *)pCameraBuffer->DescVirAddr[i];
        CurDesPhysical = (XLLP_DMAC_DESCRIPTOR_T *)pCameraBuffer->DescPhyAddr[i];
        for (j = 0; j < pCameraCxt->UsedFrames; j++) {
            for (k = 0; k < pCameraBuffer->FrameDescNum[i]; k++) {
                cprintf("DMA Descriptor 0x%08x"\
                    "(Channel %d, Frame %d, Index %d):\n",
                    CurDesPhysical, i, j, k);
                cprintf("DDADR: 0x%08x\n", CurDesVirtual->DDADR);
                cprintf("DSADR: 0x%08x\n", CurDesVirtual->DSADR);
                cprintf("DTADR: 0x%08x\n", CurDesVirtual->DTADR);
                cprintf("DCMD: 0x%08x\n", CurDesVirtual->DCMD);
                cprintf("\n");
                CurDesPhysical++;
                CurDesVirtual++;
            }
        }
    }
}

static void CamDrv_DumpImageFrameInfo(Camera_ImageFrame* pFrame)
{
    cprintf("The image frame info\n");
    cprintf("nID            = %d\n",    pFrame->nID);
    cprintf("FrameSize      = %d\n",    pFrame->FrameSize);
    cprintf("pBuffer        = 0x%x\n",  pFrame->pBuffer);
    cprintf("\n");
}

#endif

#ifdef USEIPP

// There are 22 WMMX registers should be stored:
// 16 * 64bits Data Registers
// 6 * 32bits Control Registers
#define MMX_SIZE                (0x98)
EXTERN_C unsigned long *g_pMmxData;
unsigned long *g_pMmxData = NULL;
static unsigned char s_mmxData[MMX_SIZE + 4];

EXTERN_C ECode IppYCbCr422ResizeCopy(
    unsigned char * pYSrc, unsigned char * pCbSrc, unsigned char * pCrSrc,
    unsigned char * pYDst,  unsigned char * pCbDst, unsigned char * pCrDst,
    int srcWidth, int srcHeight,
    int dstWidth, int dstHeight);

#else

// A very fast rotation function for YCbCr422 image to YCbCr420 image.
// From width * height to height * width.
// NOTE: width &  height must be multiple of 8!
static void RotateCopyYCbCr422ToYCbCr420Image(
    Byte *pDstY, Byte *pDstCb, Byte *pDstCr,
    Byte *pSrcY, Byte *pSrcCb, Byte *pSrcCr,
    int width, int height)
{
    unsigned long * pu32SrcY =(unsigned long *)pSrcY;
    unsigned long * pu32SrcCb =(unsigned long *)pSrcCb;
    unsigned long * pu32SrcCr =(unsigned long *)pSrcCr;

    unsigned long * pu32DstY;
    unsigned long * pu32DstCb;
    unsigned long * pu32DstCr;

//    UInt32 before = OSCR0;

    //rotate and copy
    int i, j;
    int work_width = width >> 2, work_height = height >> 2;

    // To improve efficiency, add many local variables for store temp results!
    int work_height_2 = work_height << 1;
    int work_height_3 = work_height * 3;
    int work_height_5 = work_height * 5;
    int work_halfheight = work_height >> 1;
    int work_halfheight_3 = work_halfheight * 3;
    int work_halfheight_5 = work_halfheight * 5;

    // Y Channel
    pu32DstY = ((unsigned long *)pDstY) + work_height - 1;
    for (j = 0; j < work_width; j++) {
        unsigned long *pY0, *pY1, *pY2, *pY3;
        unsigned long Y0, Y1, Y2, Y3;
        pY3 = pu32SrcY + j - work_width; // Dummy value to produce the initial pY0 value
        for (i = 0; i < work_height; i++) {
            pY0 = pY3 + work_width;
            pY1 = pY0 + work_width;
            pY2 = pY1 + work_width;
            pY3 = pY2 + work_width;
            Y0 = *pY0;
            Y1 = *pY1;
            Y2 = *pY2;
            Y3 = *pY3;
            *pu32DstY  = (Y3 & 0xff) | ((Y2 & 0xff) << 8) |
                ((Y1 & 0xff) << 16 ) | ((Y0 & 0xff) << 24);
            *(pu32DstY + work_height) = ((Y3 & 0xff00) >> 8) | ((Y2 & 0xff00)) |
                ((Y1 & 0xff00) << 8 ) | ((Y0 & 0xff00) << 16);
            *(pu32DstY + work_height_2) = ((Y3 & 0xff0000) >> 16) |
                ((Y2 & 0xff0000) >> 8) | ((Y1 & 0xff0000)) | ((Y0 & 0xff0000) << 8);
            *(pu32DstY + work_height_3) = ((Y3 & 0xff000000) >> 24) |
                ((Y2 & 0xff000000) >> 16) | ((Y1 & 0xff000000) >> 8) |
                ((Y0 & 0xff000000));

            pu32DstY--;
        }
        pu32DstY += work_height_5;
    }

    // Cb & Cr Channel
    pu32DstCb = ((unsigned long *)pDstCb) + work_halfheight - 1;
    pu32DstCr = ((unsigned long *)pDstCr) + work_halfheight - 1;
    for (j = 0; j < (work_width >> 1); j++) {
        unsigned long *pCb0, *pCb1, *pCb2, *pCb3;
        unsigned long *pCr0, *pCr1, *pCr2, *pCr3;
        unsigned long Cb0, Cb1, Cb2, Cb3;
        unsigned long Cr0, Cr1, Cr2, Cr3;

        pCb3 = pu32SrcCb + j - work_width; // Dummy value to produce the initial pCb0 value
        pCr3 = pu32SrcCr + j - work_width; // Dummy value to produce the initial pCr0 value
        for (i = 0; i < work_halfheight; i++) {
            // Cb channel
            pCb0 = pCb3 + work_width;
            pCb1 = pCb0 + work_width;
            pCb2 = pCb1 + work_width;
            pCb3 = pCb2 + work_width;
            Cb0 = *pCb0;
            Cb1 = *pCb1;
            Cb2 = *pCb2;
            Cb3 = *pCb3;
            *pu32DstCb =
                (Cb3 & 0xff) | ((Cb2 & 0xff) << 8) |
                ((Cb1 & 0xff) << 16 ) | ((Cb0 & 0xff) << 24);
            *(pu32DstCb + work_halfheight) =
                ((Cb3 & 0xff00) >> 8) | ((Cb2 & 0xff00)) |
                ((Cb1 & 0xff00) << 8 ) | ((Cb0 & 0xff00) << 16);
            *(pu32DstCb + work_height) =
                ((Cb3 & 0xff0000) >> 16) | ((Cb0 & 0xff0000) >> 8) |
                ((Cb1 & 0xff0000)) | ((Cb3 & 0xff0000) << 8);
            *(pu32DstCb + work_halfheight_3) =
                ((Cb3 & 0xff000000) >> 24) | ((Cb2 & 0xff000000) >> 16) |
                ((Cb1 & 0xff000000) >> 8) | ((Cb0 & 0xff000000));

            pu32DstCb--;

            // Cr channel
            pCr0 = pCr3 + work_width;
            pCr1 = pCr0 + work_width;
            pCr2 = pCr1 + work_width;
            pCr3 = pCr2 + work_width;
            Cr0 = *pCr0;
            Cr1 = *pCr1;
            Cr2 = *pCr2;
            Cr3 = *pCr3;
            *pu32DstCr =
                (Cr3 & 0xff) | ((Cr2 & 0xff) << 8) |
                ((Cr1 & 0xff) << 16 ) | ((Cr0 & 0xff) << 24);
            *(pu32DstCr + work_halfheight) =
                 ((Cr3 & 0xff00) >> 8) | ((Cr2 & 0xff00)) |
                ((Cr1 & 0xff00) << 8 ) | ((Cr0 & 0xff00) << 16);
            *(pu32DstCr + work_height) =
                ((Cr3 & 0xff0000) >> 16) | ((Cr2 & 0xff0000) >> 8) |
                ((Cr1 & 0xff0000)) | ((Cr0 & 0xff0000) << 8);
            *(pu32DstCr + work_halfheight_3) =
                ((Cr3 & 0xff000000) >> 24) | ((Cr2 & 0xff000000) >> 16) |
                ((Cr1 & 0xff000000) >> 8) | ((Cr0 & 0xff000000));

            pu32DstCr--;
        }
        pu32DstCb += work_halfheight_5;
        pu32DstCr += work_halfheight_5;
    }

//    UInt32 after = OSCR0;
//    cprintf("RotateCopyYCbCr422ToYCbCr420Image %dms\n", (after - before) / 3250);
}

#ifdef USE_YCbCr422_PREVIEW
// A very fast rotation function for YCbCr422 image.
// From width * height to height * width.
// NOTE: width &  height must be multiple of 8!
static void RotateCopyYCbCr422Image(Byte *pDstY, Byte *pDstCb, Byte *pDstCr,
    Byte *pSrcY, Byte *pSrcCb, Byte *pSrcCr,
    int width, int height)
{
    unsigned long * pu32SrcY =(unsigned long *)pSrcY;
    unsigned long * pu32SrcCb =(unsigned long *)pSrcCb;
    unsigned long * pu32SrcCr =(unsigned long *)pSrcCr;

    unsigned long * pu32DstY;
    unsigned long * pu32DstCb;
    unsigned long * pu32DstCr;

//    UInt32 before = OSCR0;

    //rotate and copy
    int i, j;
    int work_width = width >> 2, work_height = height >> 2;

    // To improve efficiency, add many local variables for store temp results!
    int work_height_2 = work_height << 1;
    int work_height_3 = work_height * 3;
    int work_height_5 = work_height * 5;
    int work_halfheight = work_height >> 1;
    int work_halfheight_3 = work_halfheight * 3;
    int work_halfheight_5 = work_halfheight * 5;
    int work_halfheight_7 = work_halfheight * 7;
    int work_halfheight_9 = work_halfheight * 9;

    // Y Channel
    pu32DstY = ((unsigned long *)pDstY) + work_height - 1;
    for (j = 0; j < work_width; j++) {
        unsigned long *pY0, *pY1, *pY2, *pY3;
        unsigned long Y0, Y1, Y2, Y3;
        pY3 = pu32SrcY + j - work_width; // Dummy value to produce the initial pY0 value
        for (i = 0; i < work_height; i++) {
            pY0 = pY3 + work_width;
            pY1 = pY0 + work_width;
            pY2 = pY1 + work_width;
            pY3 = pY2 + work_width;
            Y0 = *pY0;
            Y1 = *pY1;
            Y2 = *pY2;
            Y3 = *pY3;
            *pu32DstY  = (Y3 & 0xff) | ((Y2 & 0xff) << 8) |
                ((Y1 & 0xff) << 16 ) | ((Y0 & 0xff) << 24);
            *(pu32DstY + work_height) = ((Y3 & 0xff00) >> 8) | ((Y2 & 0xff00)) |
                ((Y1 & 0xff00) << 8 ) | ((Y0 & 0xff00) << 16);
            *(pu32DstY + work_height_2) = ((Y3 & 0xff0000) >> 16) |
                ((Y2 & 0xff0000) >> 8) | ((Y1 & 0xff0000)) | ((Y0 & 0xff0000) << 8);
            *(pu32DstY + work_height_3) = ((Y3 & 0xff000000) >> 24) |
                ((Y2 & 0xff000000) >> 16) | ((Y1 & 0xff000000) >> 8) |
                ((Y0 & 0xff000000));

            pu32DstY--;
        }
        pu32DstY += work_height_5;
    }

    // Cb & Cr Channel
    pu32DstCb = ((unsigned long *)pDstCb) + work_halfheight - 1;
    pu32DstCr = ((unsigned long *)pDstCr) + work_halfheight - 1;
    for (j = 0; j < (work_width >> 1); j++) {
        unsigned long *pCb0, *pCb1, *pCb2, *pCb3;
        unsigned long *pCr0, *pCr1, *pCr2, *pCr3;
        unsigned long Cb0, Cb1, Cb2, Cb3;
        unsigned long Cr0, Cr1, Cr2, Cr3;

        pCb3 = pu32SrcCb + j - work_width; // Dummy value to produce the initial pCb0 value
        pCr3 = pu32SrcCr + j - work_width; // Dummy value to produce the initial pCr0 value
        for (i = 0; i < work_halfheight; i++) {
            // Cb channel
            pCb0 = pCb3 + work_width;
            pCb1 = pCb0 + work_width;
            pCb2 = pCb1 + work_width;
            pCb3 = pCb2 + work_width;
            Cb0 = *pCb0;
            Cb1 = *pCb1;
            Cb2 = *pCb2;
            Cb3 = *pCb3;
            *pu32DstCb = *(pu32DstCb + work_halfheight) =
                (Cb3 & 0xff) | ((Cb2 & 0xff) << 8) |
                ((Cb1 & 0xff) << 16 ) | ((Cb0 & 0xff) << 24);
            *(pu32DstCb + work_height) = *(pu32DstCb + work_halfheight_3) =
                ((Cb3 & 0xff00) >> 8) | ((Cb2 & 0xff00)) |
                ((Cb1 & 0xff00) << 8 ) | ((Cb0 & 0xff00) << 16);
            *(pu32DstCb + work_height_2) = *(pu32DstCb + work_halfheight_5) =
                ((Cb3 & 0xff0000) >> 16) | ((Cb0 & 0xff0000) >> 8) |
                ((Cb1 & 0xff0000)) | ((Cb3 & 0xff0000) << 8);
            *(pu32DstCb + work_height_3) = *(pu32DstCb + work_halfheight_7) =
                ((Cb3 & 0xff000000) >> 24) | ((Cb2 & 0xff000000) >> 16) |
                ((Cb1 & 0xff000000) >> 8) | ((Cb0 & 0xff000000));

            pu32DstCb--;

            // Cr channel
            pCr0 = pCr3 + work_width;
            pCr1 = pCr0 + work_width;
            pCr2 = pCr1 + work_width;
            pCr3 = pCr2 + work_width;
            Cr0 = *pCr0;
            Cr1 = *pCr1;
            Cr2 = *pCr2;
            Cr3 = *pCr3;
            *pu32DstCr = *(pu32DstCr + work_halfheight) =
                (Cr3 & 0xff) | ((Cr2 & 0xff) << 8) |
                ((Cr1 & 0xff) << 16 ) | ((Cr0 & 0xff) << 24);
            *(pu32DstCr + work_height) = *(pu32DstCr + work_halfheight_3) =
                 ((Cr3 & 0xff00) >> 8) | ((Cr2 & 0xff00)) |
                ((Cr1 & 0xff00) << 8 ) | ((Cr0 & 0xff00) << 16);
            *(pu32DstCr + work_height_2) = *(pu32DstCr + work_halfheight_5) =
                ((Cr3 & 0xff0000) >> 16) | ((Cr2 & 0xff0000) >> 8) |
                ((Cr1 & 0xff0000)) | ((Cr0 & 0xff0000) << 8);
            *(pu32DstCr + work_height_3) = *(pu32DstCr + work_halfheight_7) =
                ((Cr3 & 0xff000000) >> 24) | ((Cr2 & 0xff000000) >> 16) |
                ((Cr1 & 0xff000000) >> 8) | ((Cr0 & 0xff000000));

            pu32DstCr--;
        }
        pu32DstCb += work_halfheight_9;
        pu32DstCr += work_halfheight_9;
    }

//    UInt32 after = OSCR0;
//    cprintf("RotateCopyYCbCr422Image %dms\n", (after - before) / 3250);
}
#endif

#endif

static void DMAFinishOneFrameStillCapture(PCamDrv_Context pCameraCxt)
{
    if (StillDmaTransFlag == DMA_ALL_CHANNELS_FINISH) {

        DBGOUT(cprintf("DMAFinishOneFrameStillCapture\n"));

        StillDmaTransFlag = 0;

        // BugBug: Should send images to overlay2 for preview?

        frameCount++;

        // Wait 2 Vsync, capture the third frame
        if (frameCount == pCameraCxt->UsedFrames) {
            // stop dma
            DMAStopTransfer(pCameraCxt);
            CIStopCapture();
            s_stillSnapEvent.NotifyByIsr();
        }
    }
}

static void DMAFinishOneFrameVideoCapture(PCamDrv_Context pCameraCxt)
{
//    cprintf("@");
    if (StillDmaTransFlag == DMA_ALL_CHANNELS_FINISH) {
        PCamDrv_DMABuffer pCameraBuffer = &pCameraCxt->VideoBuffer;

//        DBGOUT(cprintf("DMAFinishOneFrameVideoCapture\n"));
        StillDmaTransFlag = 0;

#ifdef USEIPP
        // For COLOR_YCbCr422
        IppYCbCr422ResizeCopy(
            (Byte *)pCameraBuffer->BufVirAddrKernel[frameCount][0],
            (Byte *)pCameraBuffer->BufVirAddrKernel[frameCount][1],
            (Byte *)pCameraBuffer->BufVirAddrKernel[frameCount][2],
            (Byte *)pCameraCxt->pOverlay2->pChannelFb[0],
            (Byte *)pCameraCxt->pOverlay2->pChannelFb[1],
            (Byte *)pCameraCxt->pOverlay2->pChannelFb[2],
            pCameraCxt->Resolution.VideoWidth,
            pCameraCxt->Resolution.VideoHeight,
            pCameraCxt->pOverlay2->Width, pCameraCxt->pOverlay2->Height);
#else
     if (s_pRotateCopy) {
        s_pRotateCopy(
                (Byte *)pCameraCxt->pOverlay2->pChannelFb[0],
                (Byte *)pCameraCxt->pOverlay2->pChannelFb[1],
                (Byte *)pCameraCxt->pOverlay2->pChannelFb[2],
                (Byte *)pCameraBuffer->BufVirAddrKernel[frameCount][0],
                (Byte *)pCameraBuffer->BufVirAddrKernel[frameCount][1],
                (Byte *)pCameraBuffer->BufVirAddrKernel[frameCount][2],
                pCameraCxt->Resolution.VideoWidth,
                pCameraCxt->Resolution.VideoHeight);
    }
#endif

        if (pCameraCxt->CaptureStatus == CamDrv_Status_Video) {
            if (pCameraCxt->OutFrameID + pCameraCxt->UsedFrames > pCameraCxt->InFrameID) {
                pCameraCxt->InFrameID++;
                DzNotifyEventObjByIsr(s_pFrameCopyEvent, SIGNALED);
            }
        }

        frameCount = (frameCount + 1) % pCameraCxt->UsedFrames;
    }
}

static Boolean _DmaChYIsr(PVoid pDev, PBoolean pWakeUp)
{
#if (_TEST_TYPE == 41)
    UInt32 uTimerStart = OSCR0;
#endif //_TEST_TYPE == 41
    PCamDrv_Context pCameraCxt = (PCamDrv_Context)pDev;
    unsigned long nDmaChY = pCameraCxt->DMAChannels[0];

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if (DCSR(nDmaChY) & XLLP_DMAC_DCSR_END_INTR) {
        DCSR(nDmaChY) |= XLLP_DMAC_DCSR_END_INTR;

//        DBGOUT(cprintf("_DmaChYIsr\n"));
        StillDmaTransFlag |= DMA_CHANNEL0_FINISH;
        if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
            if (frameCount <= pCameraCxt->UsedFrames) {
//                StillDmaTransFlag |= DMA_CHANNEL0_FINISH;
                DMAFinishOneFrameStillCapture(pCameraCxt);
            }
        }
        else {
            DMAFinishOneFrameVideoCapture(pCameraCxt);
        }
    }
    else if (DCSR(nDmaChY) & XLLP_DMAC_DCSR_BUS_ERR_INTR) {
        DCSR(nDmaChY) |= XLLP_DMAC_DCSR_BUS_ERR_INTR;
    }

#if (_TEST_TYPE == 41)
    UInt32 uTimerEnd = OSCR0;
    if (n_DmaChYIsrSampleCounter >= SAMPLETIMES) n_DmaChYIsrSampleCounter = 0;
    ul_DmaChYIsrTime[n_DmaChYIsrSampleCounter++] = \
                            (uTimerEnd >= uTimerStart) ? \
                            (uTimerEnd - uTimerStart) : \
                            ((UInt32)0xFFFFFFFF - uTimerStart + uTimerEnd);
#endif //_TEST_TYPE == 41

    return TRUE;
}

static Boolean _DmaChCbIsr(PVoid pDev, PBoolean pWakeUp)
{
#if (_TEST_TYPE == 41)
    UInt32 uTimerStart = OSCR0;
#endif //_TEST_TYPE == 41
    PCamDrv_Context pCameraCxt = (PCamDrv_Context)pDev;
    unsigned long nDmaChCb = pCameraCxt->DMAChannels[1];

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if (DCSR(nDmaChCb) & XLLP_DMAC_DCSR_END_INTR) {
        DCSR(nDmaChCb) |= XLLP_DMAC_DCSR_END_INTR;

//        DBGOUT(cprintf("_DmaChCbIsr\n"));
        StillDmaTransFlag |= DMA_CHANNEL1_FINISH;
        if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
            if (frameCount <= pCameraCxt->UsedFrames) {
                DMAFinishOneFrameStillCapture(pCameraCxt);
            }
        }
        else {
            DMAFinishOneFrameVideoCapture(pCameraCxt);
        }
    }
    else if (DCSR(nDmaChCb) & XLLP_DMAC_DCSR_BUS_ERR_INTR) {
        DCSR(nDmaChCb) |= XLLP_DMAC_DCSR_BUS_ERR_INTR;
    }
#if (_TEST_TYPE == 41)
    UInt32 uTimerEnd = OSCR0;
    if (n_DmaChCbIsrSampleCounter >= SAMPLETIMES) n_DmaChCbIsrSampleCounter = 0;
    ul_DmaChCbIsrTime[n_DmaChCbIsrSampleCounter++] = \
                            (uTimerEnd >= uTimerStart) ? \
                            (uTimerEnd - uTimerStart) : \
                            ((UInt32)0xFFFFFFFF - uTimerStart + uTimerEnd);
#endif //_TEST_TYPE == 41
    return TRUE;
}

static Boolean _DmaChCrIsr(PVoid pDev, PBoolean pWakeUp)
{
#if (_TEST_TYPE == 41)
    UInt32 uTimerStart = OSCR0;
#endif //_TEST_TYPE == 41
    PCamDrv_Context pCameraCxt = (PCamDrv_Context)pDev;
    unsigned long nDmaChCr = pCameraCxt->DMAChannels[2];

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if (DCSR(nDmaChCr) & XLLP_DMAC_DCSR_END_INTR) {
        DCSR(nDmaChCr) |= XLLP_DMAC_DCSR_END_INTR;

//        DBGOUT(cprintf("_DmaChCrIsr\n"));
        StillDmaTransFlag |= DMA_CHANNEL2_FINISH;
        if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
            if (frameCount <= pCameraCxt->UsedFrames) {
                DMAFinishOneFrameStillCapture(pCameraCxt);
            }
        }
        else {
            DMAFinishOneFrameVideoCapture(pCameraCxt);
        }
    }
    else if (DCSR(nDmaChCr) & XLLP_DMAC_DCSR_BUS_ERR_INTR) {
        DCSR(nDmaChCr) |= XLLP_DMAC_DCSR_BUS_ERR_INTR;
    }
#if (_TEST_TYPE == 41)
    UInt32 uTimerEnd = OSCR0;
    if (n_DmaChCrIsrSampleCounter >= SAMPLETIMES) n_DmaChCrIsrSampleCounter = 0;
    ul_DmaChCrIsrTime[n_DmaChCrIsrSampleCounter++] = \
                            (uTimerEnd >= uTimerStart) ? \
                            (uTimerEnd - uTimerStart) : \
                            ((UInt32)0xFFFFFFFF - uTimerStart + uTimerEnd);
#endif //_TEST_TYPE == 41
    return TRUE;
}

EXTERN_C void DMAStartTransfer(PCamDrv_Context pCameraCxt)
{
    unsigned int i;
    PCamDrv_DMABuffer pCameraBuffer;

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
        pCameraBuffer = &pCameraCxt->StillBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->VideoBuffer;
    }

    frameCount = 0;

    DRCMRCI0 = XLLP_DMAC_DRCMR_ENABLE | pCameraCxt->DMAChannels[0];
    DRCMRCI1 = XLLP_DMAC_DRCMR_ENABLE | pCameraCxt->DMAChannels[1];
    DRCMRCI2 = XLLP_DMAC_DRCMR_ENABLE | pCameraCxt->DMAChannels[2];

    // start channel 0 ~ 2
    for (i = 0; i < TRANS_CHANNELS; i++) {
        DDADR(pCameraCxt->DMAChannels[i]) = pCameraBuffer->DescPhyAddr[i];
        DCSR(pCameraCxt->DMAChannels[i]) |= XLLP_DMAC_DCSR_RUN;
    }
}

EXTERN_C void DMAStopTransfer(PCamDrv_Context pCameraCxt)
{
    frameCount = 0;

    DCSR(pCameraCxt->DMAChannels[0]) &= ~XLLP_DMAC_DCSR_RUN;
    DCSR(pCameraCxt->DMAChannels[1]) &= ~XLLP_DMAC_DCSR_RUN;
    DCSR(pCameraCxt->DMAChannels[2]) &= ~XLLP_DMAC_DCSR_RUN;
}

EXTERN_C void DMAAllocChannel(PCamDrv_Context pCameraCxt)
{
        //init context status
    pCameraCxt->DMAChannels[0] = Drv_DmaInstall(0, (void *)pCameraCxt,
        (void *)_DmaChYIsr);
    pCameraCxt->DMAChannels[1] = Drv_DmaInstall(0, (void *)pCameraCxt,
        (void *)_DmaChCbIsr);
    pCameraCxt->DMAChannels[2] = Drv_DmaInstall(0, (void *)pCameraCxt,
        (void *)_DmaChCrIsr);

    cprintf("DMA Channels: Y %d, Cb %d, Cr %d\n",
         pCameraCxt->DMAChannels[0],
         pCameraCxt->DMAChannels[1],
         pCameraCxt->DMAChannels[2]);
}

static void DMAUpdateChannelChain(PCamDrv_Context pCameraCxt,
    unsigned int ChanIdx, unsigned long FifoPhyAddr, unsigned long DmaBurstSize)
{
    XLLP_DMAC_DESCRIPTOR_T *CurDesVirtual, *CurDesPhysical;
    XLLP_DMAC_DESCRIPTOR_T *LastDesVirtual = NULL;
    int DesTransferSize = 0, RemainSize;
    unsigned int i, j;
    unsigned long TargetPhysical;
    PCamDrv_DMABuffer pCameraBuffer;

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still
        || pCameraCxt->CaptureStatus == CamDrv_Status_Video);

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
        pCameraBuffer = &pCameraCxt->StillBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->VideoBuffer;
    }

    // generate fifo0 dma chains
    CurDesVirtual = (XLLP_DMAC_DESCRIPTOR_T*)
        pCameraBuffer->DescVirAddr[ChanIdx];
    CurDesPhysical = (XLLP_DMAC_DESCRIPTOR_T*)
        pCameraBuffer->DescPhyAddr[ChanIdx];

    for (i = 0; i < pCameraCxt->UsedFrames; i++) {
        // in each iteration, generate one dma chain for one frame
        RemainSize = pCameraBuffer->FrameBufSize[ChanIdx];

        // assume the blocks are stored consecutively
        TargetPhysical = pCameraBuffer->BufPhyAddr[i][ChanIdx];

        for (j = 0; j < pCameraBuffer->FrameDescNum[ChanIdx]; j++) {
            // set descriptor
            if (RemainSize > SINGLE_DESCRIPTOR_TRANSFER_MAX)
                DesTransferSize = SINGLE_DESCRIPTOR_TRANSFER_MAX;
            else
                DesTransferSize = RemainSize;
            CurDesVirtual->DDADR = (unsigned long)CurDesPhysical +
                sizeof(XLLP_DMAC_DESCRIPTOR_T);

            CurDesVirtual->DSADR = FifoPhyAddr; // FIFO0~FIFO2 physical address
            CurDesVirtual->DTADR = TargetPhysical;
            CurDesVirtual->DCMD = DesTransferSize | XLLP_DMAC_DCMD_FLOW_SRC |
                     XLLP_DMAC_DCMD_INC_TRG_ADDR | (DmaBurstSize << 16);

            // advance pointers
            RemainSize -= DesTransferSize;
            CurDesPhysical++;
            CurDesVirtual++;
            TargetPhysical += DesTransferSize;
        }

        // stop the dma transfer on one frame captured
        LastDesVirtual = CurDesVirtual - 1;
        LastDesVirtual->DCMD = DesTransferSize |XLLP_DMAC_DCMD_END_IRQ_EN |
            XLLP_DMAC_DCMD_FLOW_SRC | XLLP_DMAC_DCMD_INC_TRG_ADDR |
            (DmaBurstSize << 16);
    }

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
        LastDesVirtual->DDADR = 0;
    }
    else {
        LastDesVirtual->DDADR =
            ((unsigned int)pCameraBuffer->DescPhyAddr[ChanIdx]);
    }
}

// Fill DMA descriptors
static void DMAUpdateChain(PCamDrv_Context pCameraCxt)
{
    DMAUpdateChannelChain(pCameraCxt, 0, CIBR0_PHY, XLLP_DMAC_BURSTSIZE_32);
    DMAUpdateChannelChain(pCameraCxt, 1, CIBR1_PHY, XLLP_DMAC_BURSTSIZE_32);
    DMAUpdateChannelChain(pCameraCxt, 2, CIBR2_PHY, XLLP_DMAC_BURSTSIZE_32);
//    DMADumpDescriptors(pCameraCxt);
}

EXTERN_C ECode DMAAllocBuffer(PCamDrv_Context pCameraCxt, int type)
{
    unsigned int i;
    unsigned int CaptureWidth = 0, CaptureHeight = 0;
    unsigned int AllocBufSize, AllocDescSize, FrameBufSize;
    unsigned int BufPhyAddr, BufVirAddrKernel, BufVirAddrUser;
    unsigned int DescPhyAddr, DescVirAddr;
    CamDrv_DMABuffer *pCameraBuffer;
    ECode ec;

    if (BUFTYPE_VIDEO == type) {
        pCameraBuffer = &pCameraCxt->VideoBuffer;
        CaptureWidth = pCameraCxt->Resolution.VideoWidth;
        CaptureHeight = pCameraCxt->Resolution.VideoHeight;
    }
    else if (BUFTYPE_STILL == type) {
        pCameraBuffer = &pCameraCxt->StillBuffer;
        CaptureWidth = pCameraCxt->Resolution.StillWidth;
        CaptureHeight = pCameraCxt->Resolution.StillHeight;
    }
    else
        return E_INVALID_ARGUMENT;

    // MUST USE YCbCr422
    FrameBufSize = CaptureWidth * CaptureHeight * 2;
    AllocBufSize = RoundUp2(FrameBufSize * pCameraCxt->UsedFrames, PAGE_SIZE);

    // Alloc memory for DMA buffer
    BufVirAddrKernel = (unsigned int)Drv_AllocBuffer(AllocBufSize >> PAGE_SHIFT, 0);
    if (!BufVirAddrKernel) {
        DBGOUT(cprintf("Alloc camera buffer error!\n"));
        return E_OUT_OF_MEMORY;
    }
    BufPhyAddr = Drv_GetBufferPhysicalAddress((void *)BufVirAddrKernel);

    ec = DzIoRemap(BufVirAddrKernel, AllocBufSize, BufPhyAddr,
            0, MemoryProtection_Read, (Address *)&BufVirAddrUser);
    if (FAILED(ec)) return ec;

//    memset((void *)BufVirAddrKernel, 0, AllocBufSize);

    pCameraBuffer->AllocBufSize = AllocBufSize;
    pCameraBuffer->FrameBufSize[0] = FrameBufSize / 2;
    pCameraBuffer->FrameBufSize[1] = FrameBufSize / 4;
    pCameraBuffer->FrameBufSize[2] = FrameBufSize / 4;

    for (i = 0; i < pCameraCxt->UsedFrames; i++) {
        pCameraBuffer->BufVirAddrUser[i][0] = BufVirAddrUser +
                    i * FrameBufSize;
        pCameraBuffer->BufVirAddrKernel[i][0] = BufVirAddrKernel +
                    i * FrameBufSize;
        pCameraBuffer->BufPhyAddr[i][0] = BufPhyAddr +
                    i * FrameBufSize;

        pCameraBuffer->BufVirAddrUser[i][1] =
                    pCameraBuffer->BufVirAddrUser[i][0] +
                    pCameraBuffer->FrameBufSize[0];
        pCameraBuffer->BufVirAddrKernel[i][1] =
                    pCameraBuffer->BufVirAddrKernel[i][0] +
                    pCameraBuffer->FrameBufSize[0];
        pCameraBuffer->BufPhyAddr[i][1] =
                    pCameraBuffer->BufPhyAddr[i][0] +
                    pCameraBuffer->FrameBufSize[0];

        pCameraBuffer->BufVirAddrUser[i][2] =
                    pCameraBuffer->BufVirAddrUser[i][1] +
                    pCameraBuffer->FrameBufSize[1];
        pCameraBuffer->BufVirAddrKernel[i][2] =
                    pCameraBuffer->BufVirAddrKernel[i][1] +
                    pCameraBuffer->FrameBufSize[1];
        pCameraBuffer->BufPhyAddr[i][2] =
                    pCameraBuffer->BufPhyAddr[i][1] +
                    pCameraBuffer->FrameBufSize[1];
    }

    AllocDescSize = 0;
    for (i = 0; i < TRANS_CHANNELS; i++) {
        pCameraBuffer->FrameDescNum[i] =
                (pCameraBuffer->FrameBufSize[i] +
                SINGLE_DESCRIPTOR_TRANSFER_MAX - 1)
                / SINGLE_DESCRIPTOR_TRANSFER_MAX;
        AllocDescSize += pCameraBuffer->FrameDescNum[i] *
                pCameraCxt->UsedFrames * 16;
    }
    AllocDescSize = RoundUp2(AllocDescSize, PAGE_SIZE);

    // Alloc memory for DMA Descriptors
    DescVirAddr = (unsigned int)Drv_AllocBuffer(
            AllocDescSize >> PAGE_SHIFT, DRVBUF_SPEED_FAST);
    if (!DescVirAddr) {
        cprintf("Alloc camera descriptors error!\n");
        return E_OUT_OF_MEMORY;
    }
    DescPhyAddr = Drv_GetBufferPhysicalAddress((void *)DescVirAddr);

    memset((void *)DescVirAddr, 0, AllocDescSize);

    pCameraBuffer->AllocDescSize = AllocDescSize;
    pCameraBuffer->DescPhyAddr[0] = DescPhyAddr;
    pCameraBuffer->DescVirAddr[0] = DescVirAddr;
    pCameraBuffer->DescPhyAddr[1] = DescPhyAddr +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[0] * 16;
    pCameraBuffer->DescVirAddr[1] = DescVirAddr +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[0] * 16;
    pCameraBuffer->DescPhyAddr[2] = pCameraBuffer->DescPhyAddr[1] +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[1] * 16;
    pCameraBuffer->DescVirAddr[2] = pCameraBuffer->DescVirAddr[1] +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[1] * 16;

//    DMADumpBufferInfo(pCameraCxt, TRUE);

    return NOERROR;
}

EXTERN_C void DMAFreeBuffer(PCamDrv_Context pCameraCxt, int type)
{
    CamDrv_DMABuffer *pCameraBuffer;

    if (BUFTYPE_VIDEO == type) {
        pCameraBuffer = &pCameraCxt->VideoBuffer;
    }
    else if (BUFTYPE_STILL == type) {
        pCameraBuffer = &pCameraCxt->StillBuffer;
    }
    else
        return;

    DzIoUnmap(pCameraBuffer->BufVirAddrUser[0][0], pCameraBuffer->AllocBufSize);

    Drv_FreeBuffer((void *)pCameraBuffer->DescVirAddr[0],
        pCameraBuffer->AllocDescSize >> PAGE_SHIFT);
    Drv_FreeBuffer((void *)pCameraBuffer->BufVirAddrKernel[0][0],
            pCameraBuffer->AllocBufSize >> PAGE_SHIFT);

    // Tag for the free memory
    pCameraBuffer->AllocBufSize = 0;
    pCameraBuffer->AllocDescSize = 0;
}

#if 0
// If no DMA buffer to use, return FALSE
static Boolean DMACheckMemoryAvailable(PCamDrv_Context pCameraCxt)
{
    if ((void *)pCameraCxt->pCameraBuffer->DescVirAddr[0] == NULL ||
        (void *)pCameraCxt->pCameraBuffer->BufVirAddr[0][0] == NULL)
        return FALSE;
    return TRUE;
}
#endif

#ifdef DEBUG_CAMERA
static void CamDrv_DumpCurrentFormat(CamDrv_AppFormat *pFormat)
{
    cprintf("Dump current format information of camera:\n");
    cprintf("PhotoResolution       = %d\n",  pFormat->PhotoResolution);
    cprintf("VideoResolution       = %d\n",  pFormat->VideoResolution);
    cprintf("FrameRate             = %d\n",  pFormat->FrameRate);
    cprintf("Zoom                  = %d\n",  pFormat->Zoom);
    cprintf("Brightness            = %d\n",  pFormat->Brightness);
    cprintf("Contrast              = %d\n",  pFormat->Contrast);
    cprintf("Sharpness             = %d\n",  pFormat->Sharpness);
    cprintf("WhiteBalance          = %d\n",  pFormat->WhiteBalance);
    cprintf("ColorMode             = %d\n",  pFormat->ColorMode);
    cprintf("SceneMode             = %d\n",  pFormat->SceneMode);
    cprintf("Direction             = %d\n",  pFormat->Direction);
    cprintf("Flash                 = %d\n",  pFormat->Flash);
    cprintf("ISO                   = %d\n",  pFormat->ISO);
    cprintf("ExposureCompensation  = %d\n",  pFormat->ExposureCompensation);
    cprintf("\n");
}

static void CamDrv_DumpPhotoInfo(Camera_Photo* pPhoto)
{
    cprintf("The snapped photo info\n");
    cprintf("Number            = %d\n", pPhoto->Number);
    cprintf("SingleBufferSize  = %d\n", pPhoto->SingleBufferSize);
    cprintf("pBuffer           = 0x%x\n", pPhoto->pBuffer);
    cprintf("\n");
}
#endif

#if 0
static Boolean CamDrv_PreCapture()
{
    Boolean bPreemption = DzDisablePreemption();

    return bPreemption;
}

static void CamDrv_PostCapture(Boolean bPreemption)
{
    // Enable interrupt of pen and perform a dummy read so that
    // the interrupt of pen can come again.

    DzRestorePreemption(bPreemption);
}
#endif

static void CamDrv_BeforeCapture(PCamDrv_Context pCameraCxt)
{
//    Boolean bPreemption = CamDrv_PreCapture();
    CIDisable(1);
    CIPrepare(pCameraCxt);
    SensorStartCapture(pCameraCxt);
    CIEnable(1);
    CIStartCapture(pCameraCxt);
    DMAStartTransfer(pCameraCxt);
//    CamDrv_PostCapture(bPreemption);
}

static void CamDrv_StopCapture(PCamDrv_Context pCameraCxt)
{
    CIDisable(1);
    CISetInterruptMask(0x3ff);
    SensorStopCapture(pCameraCxt);
    DMAStopTransfer(pCameraCxt);
    CIResetFIFO();
}

EXTERN_C ECode CamDrv_StartStillCapture(PCamDrv_Context pCameraCxt)
{
    ECode ec = NOERROR;

    // Get overlay2 information
    pCameraCxt->pOverlay2 = GetDisplayOverlayFormat(2);
    if (!pCameraCxt->pOverlay2->Enable) {
        DBGOUT(cprintf("Overlay2 isn't set, camera should not be opened!\n"));
        return E_ACCESS_DENIED;
    }

//    if (!DMACheckMemoryAvailable(pCameraCxt)) return E_OUT_OF_MEMORY;
    pCameraCxt->UsedFrames = pCameraCxt->AppFormat.ContinuousSnap;

    if (pCameraCxt->StillBuffer.AllocBufSize == 0) {
        ec = DMAAllocBuffer(pCameraCxt, BUFTYPE_STILL);
        if (FAILED(ec)) return ec;
        DMAUpdateChain(pCameraCxt);
//      DMADumpDescriptors(pCameraCxt);
    }

//    SensorSetExposureCompensation(-1); // test
    CamDrv_BeforeCapture(pCameraCxt);

    WaitResult wr;
    s_stillSnapEvent.TryWait(MAX_WAIT_TIME, &wr, NULL);
    if (WaitResult_TimedOut == wr) {
        // No buffers to use after waiting, perhaps the device isn't open.
        DBGOUT(cprintf("Time is out when getting still photo.\n"));
        ec = E_TIMED_OUT;
    }

    SensorStopCapture(pCameraCxt);

    return ec;
}

EXTERN_C void CamDrv_StopStillCapture(PCamDrv_Context pCameraCxt)
{
    CamDrv_StopCapture(pCameraCxt);
}

EXTERN_C ECode CamDrv_StartVideoCapture(PCamDrv_Context pCameraCxt)
{
    ECode ec = NOERROR;
    unsigned int format;

    if (pCameraCxt->Resolution.VideoWidth < 8 ||
        pCameraCxt->Resolution.VideoHeight < 8) {
        return E_INVALID_ARGUMENT; // Preview window too small!
    }

    if (pCameraCxt->VideoResLevel != RES_QVGA) {
        // If use the resolution less than RES_QVGA,
        // change overlay width & height internally
        OverlayFormat TempFormat;
        TempFormat.Enable = TRUE;
        TempFormat.Color = COLOR_YCbCr420; // MUST
        TempFormat.Layer = 2; // Use overlay2
        TempFormat.Bpp = 12; // YCrCb420
        TempFormat.Flag = 1; // overlay mode
        // NOTE the frame should be rotated when displaying
        TempFormat.Width = pCameraCxt->Resolution.VideoHeight;
        TempFormat.Height = pCameraCxt->Resolution.VideoWidth;
        TempFormat.PosX = (LCD_WIDTH - TempFormat.Width) / 2;
        TempFormat.PosY = (LCD_HEIGHT - TempFormat.Height) / 2;

        if (!Display_ChangeOverlay2Format(FALSE, &TempFormat)) {
            return E_ACCESS_DENIED;
        }
    }

    // Get overlay2 information
    pCameraCxt->pOverlay2 = GetDisplayOverlayFormat(2);
    if (!pCameraCxt->pOverlay2->Enable) {
        cprintf("Overlay2 isn't set, camera should not be opened!\n");
        return E_ACCESS_DENIED;
    }
    format = pCameraCxt->pOverlay2->Color;
    if (format == COLOR_YCbCr420) {
        s_pRotateCopy = RotateCopyYCbCr422ToYCbCr420Image;
    }
#ifdef USE_YCbCr422_PREVIEW
    else if (format == COLOR_YCbCr422) {
        // Compatible with old application code.
        // It will be discarded in the future.
        s_pRotateCopy = RotateCopyYCbCr422Image;
    }
#endif
    else
        s_pRotateCopy = NULL;

    pCameraCxt->UsedFrames = pCameraCxt->CaptureStatus == CamDrv_Status_Preview ?
        DEFAULT_FRAMES : VIDEO_RECORD_FRAMES;

    // Note: Preview buffer should not be changed unless driver
    // preview resolution is changed.
    if (pCameraCxt->VideoBuffer.AllocBufSize == 0) {
        ec = DMAAllocBuffer(pCameraCxt, BUFTYPE_VIDEO);
        if (FAILED(ec)) return ec;
        DMAUpdateChain(pCameraCxt);
    }

    CamDrv_BeforeCapture(pCameraCxt);

//    CIDumpRegisters();
    return ec;
}

EXTERN_C void CamDrv_StopVideoCapture(PCamDrv_Context pCameraCxt)
{
    CamDrv_StopCapture(pCameraCxt);

    if (pCameraCxt->VideoResLevel != RES_QVGA) {
        Display_ChangeOverlay2Format(TRUE, NULL);
    }
}

EXTERN_C void CamDrv_ReleaseFrames(PCamDrv_Context pCameraCxt)
{
    // Free buffers for photo
    DMAFreeBuffer(pCameraCxt, BUFTYPE_STILL);
}

// Invoke this function when camera driver is loading
EXTERN_C ECode CamDrv_Install(PCamDrv_Context pCameraCxt)
{
    DMAAllocChannel(pCameraCxt);

    return NOERROR;
}

EXTERN_C ECode CamDrv_OpenHardware(PCamDrv_Context pCameraCxt)
{
    ECode ec;

    //config camera GPIO
    GpioSetup();

    //capture interface init
    CIInit();

    // Configure CI according to sensor's hardware
    // master parallel with 8 data pins
    CISetMode(XLLP_CI_MODE_MP/*0*/, XLLP_CI_DATA_WIDTH8/*0x2*/);

    // enable pixel clock(sensor will provide pclock) and master clock = 26MHZ
    CISetClock(1, 1, 2600); // 26MHz

    // data sample on rising and h,vsync active high
    CISetPolarity(0, 0, 0);

    // fifo control, XLLP_CI_FIFO_THL_32 -> 32 bytes or
    // more in FIFO_0 causes a DMA request.
    CISetFIFO(0/*time out*/, XLLP_CI_FIFO_THL_32, 1, 1);

    // sensor power on and reset
    SensorPowerDown(0);
    SensorReset();
    DzDelay(10*1000);

    //initialize the I2C
    I2C_Initial();
    DzDelay(10*1000);

    //test sensor by read out product id and version
    int TimeOut = 5;
    char id = 0,rev = 0;
    do {
        int status = I2C_read_byte(OV9640_PID,&id);
        status=I2C_read_byte(OV9640_VER,&rev);

        DBGOUT(cprintf("pid:%x,rev:%x",id,rev));
        if (id != PID_OV ||
            (rev != PID_9640_REV2 && rev != PID_9640_REV3 && rev != PID_9650_REV1)) {//sean
            SensorPowerDown(1);
            SensorPowerDown(0);
            SensorReset();
            DzDelay(10*1000);
        }
        if (--TimeOut == 0)
            return E_ACCESS_DENIED;
    } while (id != PID_OV);

    ec = SensorInit(pCameraCxt);
    if (FAILED(ec)) return ec;

    DzDelay(10*1000);

//    CISetFrameRate((XLLP_CI_FRAME_CAPTURE_RATE)pCameraCxt->FrameRate);

    return ec;
}

EXTERN_C void CamDrv_CloseHardware(PCamDrv_Context pCameraCxt)
{
    SensorPowerDown(1);

    // Close I2C
    I2C_Disable();

    CIDeInit();

    GpioCloseCamera();
}

// Only for power management

EXTERN_C void CamDrv_SaveFormat(PCamDrv_Context pCameraCxt)
{
    memcpy(&pCameraCxt->SavedAppFormat, &pCameraCxt->AppFormat,
        sizeof(CamDrv_AppFormat));
}

EXTERN_C void CamDrv_ResumeFormat(PCamDrv_Context pCameraCxt)
{
    int i;
    ECode ec;
    int *pParam = (int *)(&pCameraCxt->SavedAppFormat);

    for (i = 0; i < (int)sizeof(s_fpFormatSet) / 4; i++) {
        CamDrv_SetFormatFunc pSetFormat = s_fpFormatSet[i];
        if (pSetFormat) ec = pSetFormat(pCameraCxt, *pParam);
        pParam++;
    }
}

EXTERN_C void CamDrv_SetDefaultFormat(PCamDrv_Context pCameraCxt)
{
    // Default values for current camera format
    pCameraCxt->AppFormat.PhotoResolution = RES_QVGA;
    pCameraCxt->AppFormat.VideoResolution = CAMERA_VR_LOW;
    pCameraCxt->AppFormat.FrameRate = 0; // 0 means using the default value
    pCameraCxt->AppFormat.Zoom = 1;
    pCameraCxt->AppFormat.Brightness = DEFAULT_BRIGHTNESS;
    pCameraCxt->AppFormat.Contrast = DEFAULT_CONTRAST;
    pCameraCxt->AppFormat.Sharpness = 0; // Should be revise later
    pCameraCxt->AppFormat.WhiteBalance = CAMERA_WB_AUTO;
    pCameraCxt->AppFormat.ColorMode = CAMERA_CM_COLOR;
    pCameraCxt->AppFormat.SceneMode = CAMERA_SM_AUTO;
    pCameraCxt->AppFormat.Direction = CAMERA_DR_HORIZONTAL;
    pCameraCxt->AppFormat.Flash = CAMERA_FL_CLOSED;
    pCameraCxt->AppFormat.ISO = 200;
    pCameraCxt->AppFormat.ExposureCompensation = 0;
    pCameraCxt->AppFormat.ContinuousSnap = 1;

    SensorGetResolution(pCameraCxt, RES_QVGA);
    pCameraCxt->VideoResLevel = RES_QVGA;
    pCameraCxt->StillResLevel = RES_QVGA;

    //pCameraCxt->bVideo = FALSE;
}

// Invoke this function when an application starts to use camera
EXTERN_C ECode CamDrv_Init(PCamDrv_Context pCameraCxt)
{
    ECode ec = NOERROR;

    CamDrv_SetDefaultFormat(pCameraCxt);

    // set sensor info
    pCameraCxt->SensorType = CAMERA_TYPE_OV9653;

    // Now use YCbCr422 PLANAR only
    pCameraCxt->InputFormat = XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR/*16*/;
    pCameraCxt->OutputFormat = XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR/*16*/;

    pCameraCxt->OutputFormat = XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR; // MUST
    pCameraCxt->FrameRate = 0x0;
    pCameraCxt->UsedFrames = DEFAULT_FRAMES; // DMA buffers hold DEFAULT_FRAMES frames

#ifdef USEIPP
    // s_pMmxData should be 8 bytes aligned
    g_pMmxData = (ulong_t *)(((unsigned long)s_mmxData + 4) & ~7);
#endif

    ec = CamDrv_OpenHardware(pCameraCxt);

    return ec;
}

EXTERN_C void CamDrv_Quit(PCamDrv_Context pCameraCxt)
{
    CamDrv_CloseHardware(pCameraCxt);

    // Free photo buffer if it still exists
    if (pCameraCxt->StillBuffer.AllocBufSize > 0) {
        DMAFreeBuffer(pCameraCxt, BUFTYPE_STILL);
    }

    // Free preview buffer if allocated
    if (pCameraCxt->VideoBuffer.AllocBufSize > 0) {
        DMAFreeBuffer(pCameraCxt, BUFTYPE_VIDEO);
    }
}

EXTERN_C ECode CamDrv_SetBrightness(
    PCamDrv_Context pCameraCxt, int Brightness)
{
    ECode ec;
    int level;

    // Brightness range: [0x00, 0xff]
    if (Brightness < 0 || Brightness > 255) return E_INVALID_ARGUMENT;

    level = GET_BRIGHTNESS_LEVEL(Brightness);
    ec = SensorSetBrightness(level);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.Brightness = Brightness;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetContrast(PCamDrv_Context pCameraCxt, int Contrast)
{
    ECode ec;
    int level;

    // Contrast range: [0x00, 0xff]
    if (Contrast < 0 || Contrast > 255) return E_INVALID_ARGUMENT;

    level = GET_CONTRAST_LEVEL(Contrast);
    ec = SensorSetContrast(level);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.Contrast = Contrast;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetSharpness(PCamDrv_Context pCameraCxt, int Sharpness)
{
    ECode ec;

    ec = SensorSetSharpness(Sharpness);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.Sharpness = Sharpness;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetWhiteBalance(PCamDrv_Context pCameraCxt, int WhiteBalance)
{
    ECode ec;

    ec = SensorSetWhiteBalance(WhiteBalance);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.WhiteBalance = WhiteBalance;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetColorMode(PCamDrv_Context pCameraCxt, int ColorMode)
{
    ECode ec;
    ec = SensorSetColorMode(ColorMode);

    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.ColorMode = ColorMode;
    }
    return ec;
}

EXTERN_C ECode CamDrv_SetSceneMode(PCamDrv_Context pCameraCxt, int SceneMode)
{
    ECode ec;
    ec = SensorSetSceneMode(SceneMode);

    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.SceneMode = SceneMode;
    }
    return ec;
}

EXTERN_C ECode CamDrv_SetDigitalZoom(PCamDrv_Context pCameraCxt, int zoom)
{
    ECode ec;

//    if (pCameraCxt->bVideo) return E_ACCESS_DENIED;

    if (zoom == pCameraCxt->AppFormat.Zoom) return NOERROR;

//    while (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
//        DzDelay(10000);
//    }

    ec = SensorCheckDigitalZoom(pCameraCxt, zoom); // Only check the params
    if (FAILED(ec)) return ec;

    // zoom maybe a double value, but here we only support int value
    pCameraCxt->AppFormat.Zoom = zoom;

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        // Stop & start video capture again,
        // Force the sensor resolution to reset.
        int i;
        int *pParam = (int *)(&pCameraCxt->AppFormat);

        CamDrv_StopVideoCapture(pCameraCxt);

        // Init sensor again, otherwise there may be some problem
        // when previewing.
        SensorInit(pCameraCxt);

        // Because many registers are reset using SensorInit,
        // some format should be set again to restore register value.
        for (i = 0; i < (int)sizeof(s_fpFormatSetForZoom) / 4; i++) {
            CamDrv_SetFormatFunc pSetFormat = s_fpFormatSetForZoom[i];
            if (pSetFormat) pSetFormat(pCameraCxt, *pParam);
            pParam++;
        }

        CamDrv_StartVideoCapture(pCameraCxt);
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetPhotoResolution(
    PCamDrv_Context pCameraCxt, int Resolution)
{
    ECode ec;

//    if (Resolution == pCameraCxt->AppFormat.PhotoResolution) return NOERROR;

//    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still)
//        return E_ACCESS_DENIED;

    // Reset continuous snap & digital zoom
    if (pCameraCxt->AppFormat.ContinuousSnap > 1) {
        CamDrv_SetContinuousSnap(pCameraCxt, 1);
    }
    if (pCameraCxt->AppFormat.Zoom > 1) {
        CamDrv_SetDigitalZoom(pCameraCxt, 1);
    }

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        // Stop video capture and start it again later,
        // Force the sensor resolution to reset.
        CamDrv_StopVideoCapture(pCameraCxt);
    }

    ec = SensorGetResolution(pCameraCxt, Resolution);
    if (FAILED(ec)) return ec;

    pCameraCxt->AppFormat.PhotoResolution = Resolution;

    // Buffer size maybe change if resolution is changed,
    // So free the buffer and let it is allocated again later
    if (pCameraCxt->StillBuffer.AllocBufSize > 0) {
        CamDrv_ReleaseFrames(pCameraCxt);
    }

    DBGOUT(cprintf("Current photo resolution: %d * %d\n",
            pCameraCxt->Resolution.StillWidth,
            pCameraCxt->Resolution.StillHeight));

    DBGOUT(CamDrv_DumpCurrentFormat(&pCameraCxt->AppFormat));

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        CamDrv_StartVideoCapture(pCameraCxt);
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetVideoResolution(
    PCamDrv_Context pCameraCxt, int Resolution)
{
    ECode ec;
    int DrvRes;

    if (!pCameraCxt->bVideo) return E_ACCESS_DENIED;

//    if (Resolution == pCameraCxt->AppFormat.VideoResolution) return NOERROR;

    // In driver, video resolution & photo resolution share the same defintion.
    DrvRes = VIDEORES_TO_DRVRES(Resolution);

//    if (pCameraCxt->CaptureStatus == CamDrv_Status_Still)
//        return E_ACCESS_DENIED;

    // Reset digital zoom
    // Don't use zoom in video mode now.
    if (pCameraCxt->AppFormat.Zoom > 1) {
        CamDrv_SetDigitalZoom(pCameraCxt, 1);
    }

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        // Stop video capture and start it again later,
        // Force the sensor resolution to reset.
        CamDrv_StopVideoCapture(pCameraCxt);
    }

    ec = SensorGetResolution(pCameraCxt, DrvRes);
    if (FAILED(ec)) goto Exit;

    pCameraCxt->AppFormat.VideoResolution = Resolution;

    // Buffer size maybe change if resolution is changed,
    // So free the buffer and let it is allocated again later
    if (pCameraCxt->VideoBuffer.AllocBufSize > 0) {
        Drv_Assert(pCameraCxt->VideoBuffer.AllocBufSize > 0);
        DMAFreeBuffer(pCameraCxt, BUFTYPE_VIDEO);
    }

    DBGOUT(cprintf("Current video resolution: %d * %d\n",
            pCameraCxt->Resolution.VideoWidth,
            pCameraCxt->Resolution.VideoHeight));

    DBGOUT(CamDrv_DumpCurrentFormat(&pCameraCxt->AppFormat));

Exit:
    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        CamDrv_StartVideoCapture(pCameraCxt);
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetAppFormat(PCamDrv_Context pCameraCxt,
    unsigned int nID, int wParam, int lParam)
{
    ECode ec;

    switch (nID) {
        case CAMERA_FORMAT_PHOTO_RESOLUTION:
            ec = CamDrv_SetPhotoResolution(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_VIDEO_RESOLUTION:
            ec = CamDrv_SetVideoResolution(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_ZOOM:
            ec = CamDrv_SetDigitalZoom(pCameraCxt, wParam / lParam);
            break;
        case CAMERA_FORMAT_BRIGHTNESS:
            ec = CamDrv_SetBrightness(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_CONTRAST:
            ec = CamDrv_SetContrast(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_SHARPNESS:
            ec = CamDrv_SetSharpness(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_WHITEBALANCE:
            ec = CamDrv_SetWhiteBalance(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_COLORMODE:
            ec = CamDrv_SetColorMode(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_SCENEMODE:
            ec = CamDrv_SetSceneMode(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_EXPOSURE_COMPENSATION:
            ec = CamDrv_SetExposureCompensation(pCameraCxt, wParam);
            break;
        case CAMERA_FORMAT_CONTINUOUS_SNAP:
            if (wParam <= 0) wParam = 1;
            ec = CamDrv_SetContinuousSnap(pCameraCxt, wParam);
            break;
        default:
            ec = E_NOT_IMPLEMENTED;
            break;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetExposureCompensation(
    PCamDrv_Context pCameraCxt, int nExposureCompensation)
{
    ECode ec;

    ec = SensorSetExposureCompensation(nExposureCompensation);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.ExposureCompensation = nExposureCompensation;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetContinuousSnap(
    PCamDrv_Context pCameraCxt, int nCotinuousCount)
{
    if (nCotinuousCount == pCameraCxt->AppFormat.ContinuousSnap) return NOERROR;

    if (nCotinuousCount < 1) goto Error;

    switch (pCameraCxt->AppFormat.PhotoResolution) {
        case RES_QVGA:
        // Low quality, allow take 1 ~ 16 photos continuously
            if (nCotinuousCount > 16) goto Error;
            break;
        case RES_VGA:
        // Low quality, allow take 1 ~ 4 photos continuously
            if (nCotinuousCount > 4) goto Error;
            break;
        case RES_SXGA:
        // Low quality, only allow take 1 photo at a time
            if (nCotinuousCount > 1) goto Error;
            break;
    }
    pCameraCxt->AppFormat.ContinuousSnap = nCotinuousCount;

    // Buffer size maybe change if nCotinuousCount is changed,
    // So free the buffer and let it is allocated again later
    if (pCameraCxt->StillBuffer.AllocBufSize > 0) {
        CamDrv_ReleaseFrames(pCameraCxt);
    }

    return NOERROR;
Error:
    return E_INVALID_ARGUMENT;
}

EXTERN_C ECode CamDrv_GetAppFormat(PCamDrv_Context pCameraCxt,
    unsigned int nID, int *pwParam, int *plParam)
{
    ECode ec = NOERROR;
    DBGOUT(CamDrv_DumpCurrentFormat(&pCameraCxt->AppFormat));

    switch (nID) {
        case CAMERA_FORMAT_PHOTO_RESOLUTION:
            *pwParam = pCameraCxt->AppFormat.PhotoResolution;
            break;
        case CAMERA_FORMAT_VIDEO_RESOLUTION:
            *pwParam = pCameraCxt->AppFormat.VideoResolution;
            break;
        case CAMERA_FORMAT_PHOTO_GETSIZE:
            *pwParam = pCameraCxt->Resolution.StillWidth;
            *plParam = pCameraCxt->Resolution.StillHeight;
            break;
        case CAMERA_FORMAT_VIDEO_GETSIZE:
            *pwParam = pCameraCxt->Resolution.VideoWidth;
            *plParam = pCameraCxt->Resolution.VideoHeight;
            break;
        case CAMERA_FORMAT_ZOOM:
            *pwParam = pCameraCxt->AppFormat.Zoom;
            *plParam = 1;
            break;
        case CAMERA_FORMAT_BRIGHTNESS:
            *pwParam = pCameraCxt->AppFormat.Brightness;
            break;
        case CAMERA_FORMAT_CONTRAST:
            *pwParam = pCameraCxt->AppFormat.Contrast;
            break;
        case CAMERA_FORMAT_SHARPNESS:
            *pwParam = pCameraCxt->AppFormat.Sharpness;
            break;
        case CAMERA_FORMAT_WHITEBALANCE:
            *pwParam = pCameraCxt->AppFormat.SceneMode;
            break;
        case CAMERA_FORMAT_COLORMODE:
            *pwParam = pCameraCxt->AppFormat.ColorMode;
            break;
        case CAMERA_FORMAT_SCENEMODE:
            *pwParam = pCameraCxt->AppFormat.SceneMode;
            break;
        case CAMERA_FORMAT_FLASH:
            *pwParam = pCameraCxt->AppFormat.Flash;
            break;
        case CAMERA_FORMAT_EXPOSURE_COMPENSATION:
            *pwParam = pCameraCxt->AppFormat.ExposureCompensation;
            break;
        case CAMERA_FORMAT_CONTINUOUS_SNAP:
            *pwParam = pCameraCxt->AppFormat.ContinuousSnap;
            break;
        case CAMERA_FORMAT_PHOTO_GETFRAMESIZE:
            *pwParam = pCameraCxt->Resolution.StillWidth *
            pCameraCxt->Resolution.StillHeight * 2;
            break;
        default:
            ec = E_NOT_IMPLEMENTED;
            break;
    }

    return ec;
}

// Return physical address now.
EXTERN_C ECode CamDrv_GetStillPhoto(
    PCamDrv_Context pCameraCxt, Camera_Photo* pPhoto)
{
    unsigned long PhotoWidth, PhotoHeight;
    const int BytesPerPixel = 2; // For YCbCr422

    if (pCameraCxt->CaptureStatus != CamDrv_Status_Still)
        return E_ACCESS_DENIED;

    pPhoto->Number = pCameraCxt->AppFormat.ContinuousSnap;
    PhotoWidth = pCameraCxt->Resolution.StillWidth;
    PhotoHeight = pCameraCxt->Resolution.StillHeight;

    // Get photos from DMA buffer directly

    // BufferSize may be larger than PhotoWidth * PhotoHeight * BytesPerPixel
    // But now they are actually equal
    pPhoto->SingleBufferSize = PhotoWidth * PhotoHeight * BytesPerPixel;
    pPhoto->pBuffer = (void *)pCameraCxt->StillBuffer.BufVirAddrUser[0][0];

    DBGOUT(CamDrv_DumpPhotoInfo(pPhoto));

    return NOERROR;
}

EXTERN_C ECode CamDrv_StartVideoRecord(
    PCamDrv_Context pCameraCxt, IEvent ** ppOutEvent)
{
    Drv_Assert(ppOutEvent);
    pCameraCxt->InFrameID = 0;
    pCameraCxt->OutFrameID = 0;
    DzCreateEventObj(FALSE, UNSIGNALED, &s_pFrameCopyEvent);
    s_pFrameCopyEvent->AddRef(); // For *ppOutEvent, user app release it
    *ppOutEvent = s_pFrameCopyEvent;

    // Video recording need more buffers
    if (pCameraCxt->VideoBuffer.AllocBufSize > 0) {
        DMAFreeBuffer(pCameraCxt, BUFTYPE_VIDEO);
    }

    return CamDrv_StartVideoCapture(pCameraCxt);
}

EXTERN_C void CamDrv_StopVideoRecord(PCamDrv_Context pCameraCxt)
{
    s_pFrameCopyEvent->Release();
    return CamDrv_StopVideoCapture(pCameraCxt);
}

EXTERN_C ECode CamDrv_GetVideoFrame(
    PCamDrv_Context pCameraCxt, Camera_ImageFrame* pFrame)
{
    pFrame->nID = pCameraCxt->OutFrameID;
    pFrame->FrameSize = pCameraCxt->VideoBuffer.FrameBufSize[0] +
                        pCameraCxt->VideoBuffer.FrameBufSize[1] +
                        pCameraCxt->VideoBuffer.FrameBufSize[2];
    pFrame->pBuffer =
        (void *)pCameraCxt->VideoBuffer.BufVirAddrUser
                [GET_VIDEOBUFFER_ID(pFrame->nID)][0];

    pCameraCxt->OutFrameID++;

    DBGOUT(CamDrv_DumpImageFrameInfo(pFrame));

    return NOERROR;
}
